/*
Automatic clinic tests using a robot arm.
Copyright (C) 2010  CIC-CPC, CHU Gabriel Montpied
All other trademarks, logos and copyrights are the property
of their respective owners.

This file is part of cpc-painbot.

cpc-painbot is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

cpc-painbot is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with cpc-painbot.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef CPC_PAINBOT_POSITIONBRAS_H
#define CPC_PAINBOT_POSITIONBRAS_H
#include <cmath>
#include <limits>
#include <QObject>
#include <QDebug>

class QXmlStreamReader;

/*! Position du bras.
    La position du bras est soumise à différentes contraintes :
    le bras se déplace sur un arc de cercle et possède un rayon d'action bien
    défini.
    Lorsque l'on déplace le bras dans une direction, celle-ci est d'abord
    vérifiée.
*/
class PositionBras : public QObject
{
    Q_OBJECT
    //! Forme du bras.
    Q_PROPERTY(FormeBras forme READ forme WRITE setForme);
    //! Position en x (gauche-droite), en mm.
    Q_PROPERTY(qreal x READ x WRITE setX);
    //! Position en y (avant-arrière), en mm.
    Q_PROPERTY(qreal y READ y WRITE setY);
    //! Position en z (haut-bas), en mm.
    Q_PROPERTY(qreal z READ z WRITE setZ);
    //! Rotation de la tête, en degré.
    Q_PROPERTY(qreal r READ r WRITE setR);
    //! Enumérations associées.
    Q_ENUMS(Taille Limite FormeBras);

public:
    /*! Taille des articulations, en mm. */
    enum Taille {
        TailleAvantBras = 180, //!< Longueur de l'avant-bras.
        TailleBras = 260, //!< Longueur du bras (\em épaule jusqu'au \em coude).
        TailleTotale = TailleBras + TailleAvantBras //!< Longueur totale du bras (bras et avant-bras)
    };

    /*! Limites de déplacement, en mm. */
    enum Limite {
        XMin = -TailleTotale, //!< Déplacement maximal à gauche.
        XMax = TailleTotale,  //!< Déplacement maximal à droite.
        YMin = 0,             //!< Déplacement maximal vers l'arrière.
        YMax = TailleTotale,  //!< Déplacement maximal vers l'avant.
        ZMin = 0,             //!< Déplacement maximal vers le haut.
        ZMax = 100,           //!< Déplacement maximal vers le bas.
        RMin = -180,          //!< Rotation maximale sens anti-horaire.
        RMax = 180,           //!< Rotation maximale sens horaire.
        CouronneMax = TailleTotale, //!< Plus grand diamètre sur lequel le bras peut agir.
        CouronneMin = 138           //!< Plus petit diamètre sur lequel le bras peut agir.
    };

    /*! Forme du bras.
        Le bras peut atteindre une position en mode gaucher ou
        droitier.
    */
    enum FormeBras {
        Droitier,   //!< Approche 'main droite' (\em coude du bras à droite).
        Gaucher     //!< Approche 'main gauche' (\em coude du bras à gauche).
    };

    //! Carré du rayon maximum.
    static qreal rayonMax2()   { return CouronneMax * CouronneMax; }
    //! Rayon maximum.
    static qreal rayonMax()    { return CouronneMax; }
    //! Carré du rayon minimum.
    static qreal rayonMin2()   { return CouronneMin * CouronneMin; }
    //! Rayon minimum.
    static qreal rayonMin()    { return CouronneMin; }

    /*! Chargement depuis un noeud XML.
    */
    static PositionBras *charger(QXmlStreamReader *xml, QObject *parent = 0);

    /*! Constructeur.
        \param[in] x      position en x.
        \param[in] y      position en y.
        \param[in] z      position en z.
        \param[in] r      rotation de la tête
        \param[in] forme  Forme du bras.
        \param     parent Parent de l'objet.
    */
    PositionBras(qreal x = 0, qreal y = 0, qreal z = 0,
                    qreal r = 0, FormeBras forme = Droitier, QObject * parent = 0);

    /*! Constructeur copie.
        \param[in] pos La position à reproduire.
    */
    PositionBras(const PositionBras &pos);

    /*! Affectation.
        \param[in] pos La position à reproduire.
    */
    PositionBras & operator =(const PositionBras &pos);

    //! Forme du bras.
    FormeBras forme() const { return forme_; }
    //! Coordonnée x.
    qreal x() const { return x_; }
    //! Coordonnée y.
    qreal y() const { return y_; }
    //! Coordonnée z.
    qreal z() const { return z_; }
    //! Coordonnée r.
    qreal r() const { return r_; }

    //! Définit la forme du bras.
    void setForme(FormeBras forme) { forme_ = forme; }

    //! Définit la coordonnée x.
    inline void setX(qreal x);
    //! Définit la coordonnée y.
    inline void setY(qreal y);
    //! Définit la coordonnée z.
    inline void setZ(qreal z);
    //! Définit la coordonnée r.
    inline void setR(qreal r);

    //! Déplacement de dx sur la composante x.
    void deltaX(qreal dx) { setX(x() + dx); }
    //! Déplacement de dy sur la composante y.
    void deltaY(qreal dy) { setY(y() + dy); }
    //! Déplacement de dz sur la composante z.
    void deltaZ(qreal dz) { setZ(z() + dz); }
    //! Déplacement de dr sur la composante r.
    void deltaR(qreal dr) { setR(r() + dr); }

    /*! Le carré du rayon.
        \return Le carré du rayon.
    */
    qreal rayon2() const { return (y() * y() + x() * x()); }

    /*! Fournit le rayon courant.
        Le bras se déplaçant sur un arc de cercle,
        il suit un rayon.

        \return Le rayon courant.
    */
    qreal rayon() const { return sqrt(rayon2()); }

    /*! Angle par rapport à l'origine.
        \return L'angle formé par rapport à l'axe d'origine.
    */
    qreal angle() const;

    /*! Angle du bras.
        \return L'angle formé par le bras, par rapport au rayon formé par la position courante.
        \note Pour retrouver l'angle par rapport à l'origine, il suffit d'ajouter angle() à cette
        valeur.
    */
    qreal angleBras() const;

    /*! Angle du coude.
        \return L'angle formé par le coude, entre le bras et l'avant-bras.
    */
    qreal coude() const;

    /*! Indique si les coordonnées sont valides.
        \retval true Les coordonnées sont bonnes.
        \retval false La position n'est pas atteignable.
    */
    inline bool isValide() const;

    /*! Déplace la position.
        Déplace la position courante des quotas indiqués.
        \param[in] dx Déplacement en X
        \param[in] dy Déplacement en Y
        \param[in] dz Déplacement en Z
        \param[in] dr Déplacement en R
    */
    inline void deplacer(qreal dx, qreal dy, qreal dz, qreal dr);


    /*! Ajuste la position.
        Lors d'un déplacement, il se peut que la position demandée
        ne valide pas toutes les contraintes.
        Ajuster la position va donc permettre à celle-ci d'être incluse
        dans les limites de déplacement du bras.
    */
    inline void ajuster(void);

    /*! Calcule la distance par rapport à une autre position.
        \param[in] pos La position par rapport à laquelle on calcule l'écart.

        \return La distance entre les deux positions.
    */
    inline qreal distance(const PositionBras &pos) const;

    /*! Calcule l'angle entre les têtes de deux positions.
        \param[in] pos L'autre position.

        \return L'angle entre la tête de cette position et celle de l'autre position.
    */
    qreal angleTete(const PositionBras &pos) const
    {
        return r() - pos.r();
    }

    /*! Ecart entre deux positions.
        \param[in] pos L'autre position.

        \return Une position delta.
    */
    inline PositionBras delta(const PositionBras & pos) const;

    //! Egalité entre deux positions.
    inline bool operator == (const PositionBras &pos) const;
    //! Différence entre deux positions.
    inline bool operator != (const PositionBras &pos) const;

    /*! Représentation de la position.
        Génère une chaîne de caractères qui représente la position.

        \return Une chaîne représentation la position.
    */
    inline QString generer() const;

protected:
    FormeBras forme_; //!< Forme associée à cette position.
    qreal x_; //!< Coordonnée x
    qreal y_; //!< Coordonnée y
    qreal z_; //!< Coordonnée z
    qreal r_; //!< Coordonnée r
};

/*! Calcule la distance entre deux positions. */
inline qreal distance(const PositionBras &pa, const PositionBras &pb)
{
    return pa.distance(pb);
}

// -------------------------------- Définitions Inline ---------------------------------------------
inline void PositionBras::setX(qreal nx)
{
    //qDebug() << "PositionBras::setX(" << nx << " )";
    x_ = qBound(qreal(XMin), nx, qreal(XMax));
}

inline void PositionBras::setY(qreal ny)
{
    //qDebug() << "PositionBras::setY(" << ny << " )";
    y_ = qBound(qreal(YMin), ny, qreal(YMax));
}

inline void PositionBras::setZ(qreal z)
{
    //qDebug() << "PositionBras::setZ(" << z << " )";
    z_ = qBound(qreal(ZMin), z, qreal(ZMax));
}

inline void PositionBras::setR(qreal r)
{
    //qDebug() << "PositionBras::setR(" << r << " )";
    r_ = qBound(qreal(RMin), r, qreal(RMax));
}

inline qreal PositionBras::angle() const
{
    // On recherche le calcul qui nous offrira la meilleure précision.
    // Note : L'angle étant orienté, on se base sur le signe de x, dans
    // le cas où Y est utilisé pour l'angle.
    // Note : A cause du risque d'instabilité numérique, on utilise qBound pour contraindre
    // les valeurs dans un domaine connu.
    qreal angle = 0;
    int signe = (x() / qAbs(x())) > 0 ? 1 : -1;
    if ( qFuzzyCompare(1 + qAbs(x()), 1 + std::numeric_limits<qreal>::epsilon()) )  {
        // x est proche de 0, x / |x| est donc instable.
        //qDebug() << "PositionBras, angle calculé à partir de x (car proche 0)";
        angle = asin(qBound(qreal(-1), x() / rayon(), qreal(1)));
    } else if ( y() > x() ) {
        // y, plus grand, offrira vraisemblablement une meilleure précision.
        //qDebug() << "PositionBras, angle calculé à partir de y";
        angle = signe * acos(qBound(qreal(-1), y() / rayon(), qreal(1)));
    } else {
        //qDebug() << "PositionBras, angle calculé à partir de x";
        angle = asin(qBound(qreal(-1), x() / rayon(), qreal(1)));
    }

    return angle;
}

inline qreal PositionBras::angleBras() const
{
    return acos(qBound(qreal(-1),
                    (rayon2() + TailleBras*TailleBras - TailleAvantBras*TailleAvantBras)
                    / (2 * rayon() * TailleBras),
                    qreal(1))
            );
}

inline qreal PositionBras::coude() const
{
    return acos(qBound(qreal(-1),
                    (TailleBras*TailleBras + TailleAvantBras*TailleAvantBras - rayon2())
                    / (2 * TailleBras * TailleAvantBras),
                    qreal(1))
                ) - M_PI;
}

inline bool PositionBras::isValide() const
{
    return rayonMin2() <= rayon2() && rayon2() <= rayonMax2() // appartient à la couronne
            && RMin <= r() && r() <= RMax                     // rotation de la tête
            && ZMin <= z() && z() <= ZMax;                    // déplacement vertical
}

void PositionBras::deplacer(qreal dx, qreal dy, qreal dz, qreal dr)
{
    deltaX(dx);
    deltaY(dy);
    deltaZ(dz);
    deltaR(dr);

    ajuster();
}

void PositionBras::ajuster(void)
{
    if ( !isValide() ) {
        // Rayon duquel on désire se rapprocher :
        // Le plus petit entre le rayon courant et le rayon maximum
        // Et le plus grand entre le rayon obtenu et le rayon minimum.
        qreal rayon = qBound(rayonMin(), this->rayon(), rayonMax());
        qreal angle = this->angle();
        //qDebug() << "PositionBras::ajuster : angle =" << angle;

        setX( sin(angle) * rayon );
        setY( cos(angle) * rayon );
    }
}

qreal PositionBras::distance(const PositionBras &pos) const
{
    qreal dx = x() - pos.x();
    qreal dy = y() - pos.y();
    qreal dz = z() - pos.z();

    //qDebug() << "PositionBras::distance ="
    //        << sqrt(dx*dx + dy*dy + dz*dz);

    return sqrt(dx * dx + dy * dy + dz * dz);
}

PositionBras PositionBras::delta(const PositionBras &pos) const
{
    PositionBras ecart(forme());
    ecart.setX(x() - pos.x());
    ecart.setY(y() - pos.y());
    ecart.setZ(z() - pos.z());
    ecart.setR(r() - pos.r());

    return ecart;
}

bool PositionBras::operator ==(const PositionBras &pos) const
{
    bool delta_distance = qFuzzyCompare(1 + distance(pos), 1 + std::numeric_limits<qreal>::epsilon());
    bool delta_tete = qFuzzyCompare(1 + fabs(angleTete(pos)), 1 + std::numeric_limits<qreal>::epsilon());
/*
    qDebug() << "PositionBras:: == ?"
            << "Forme :" << (forme() == pos.forme())
            << "Distance :" << delta_distance
            << "Angle Tete :" << delta_tete;
*/

    return forme() == pos.forme()
            && delta_distance
            && delta_tete;
}

bool PositionBras::operator !=(const PositionBras &pos) const
{
    return !(this->operator ==(pos));
}

QString PositionBras::generer() const
{
    QString chaine;
    QTextStream flux(&chaine);
    flux.setRealNumberNotation(QTextStream::FixedNotation);
    flux.setRealNumberPrecision(3);
    flux << forme() << ',' << x() << ',' << y() << ',' << z() << ',' << r();
    return chaine;
}
#endif // CPC_PAINBOT_POSITIONBRAS_H
